/*
  Software serial multple serial test
 
 Receives from the hardware serial, sends to software serial.
 Receives from software serial, sends to hardware serial.
 
 The circuit:
 * RX is digital pin  (connect to TX of other device)
 * TX is digital pin  (connect to RX of other device)
 
 Note:
 Not all pins on the Mega and Mega 2560 support change interrupts,
 so only the following can be used for RX:
 10, 11, 12, 13, 50, 51, 52, 53, 62, 63, 64, 65, 66, 67, 68, 69
 
 Not all pins on the Leonardo support change interrupts,
 so only the following can be used for RX:
 8, 9, 10, 11, 14 (MISO), 15 (SCK), 16 (MOSI).
 
  
 This example code is in the public domain.
 
 */
#include <SoftwareSerial.h>
#define MAESTRO_BAUDRATE         9600
#define servoControllerResetPin 5
#define servoControllerRxPin 6
#define servoControllerTxPin 7

SoftwareSerial maestroSerial(servoControllerRxPin, servoControllerTxPin); // RX, TX

char ch;

boolean PololuProtocol=true;

void setup()  
{
  
  Serial.begin(9600); 
  maestroSerial.begin(MAESTRO_BAUDRATE);  
  maestro_init_serial();  
  
}

void loop() 
{
  static int v = 0;
  static int activeServo=0;
    
  ch=0; 
  if (Serial.available())
    ch = Serial.read(); 
    switch(ch) {
      
      case '0'...'9':
      v = v * 10 + ch - '0';
      break;
        
      case 'r':
      maestro_hard_reset();
      show("Pololu resetted", 0, true);            
      break; 
      
      case 'a':
      if (maestroSerial.available()) {
          show("Pololu Serial available", 0, true);     
      } else {
          show("Pololu Serial NOT available", 0, true); 
      }      
      break; 
      
      case 'l':
      maestro_set_target(activeServo, 4000);
      break;
      
      case 'm':
      maestro_set_target(activeServo, 8000);
      break; 
      
      case 's':
      activeServo=v;
      show("active Servo now", activeServo, true);
      v=0;
      break;     
      
      case 't':
      maestro_set_target(activeServo, v);      
      show("activeServo: ", activeServo, true);
      show("Target set to: ", v, true);
      v=0;
      break;
      
    }
    
}

void maestro_hard_reset() {  
  digitalWrite(servoControllerResetPin, LOW); 
  delay(100);
  digitalWrite(servoControllerResetPin, HIGH);
}


void maestro_init_serial(){  
  pinMode(servoControllerRxPin, INPUT);
  digitalWrite(servoControllerTxPin, HIGH); //keeps pololu board from getting spurious signal. Serial is idle high.
  pinMode(servoControllerTxPin, OUTPUT);
  pinMode(servoControllerResetPin,OUTPUT);
  maestro_hard_reset();
  maestroSerial.begin(MAESTRO_BAUDRATE); 
  delay(5); 
}

void show(char nameStr[], double val, boolean newline) {
  Serial.print(nameStr);  
  Serial.print("=");
  if (newline)
       Serial.println(val);
  else Serial.print(val);
}



void maestro_set_target(byte servo, int target) {  
    maestroSerial.write(0xAA); //start byte
    maestroSerial.write(12); //device id (assuming it is still at the default value)
    maestroSerial.write(0x04); //command number with MSB cleared
    maestroSerial.write(servo); //servo number
    maestroSerial.write(target & 0x7F);
    maestroSerial.write((target >> 7) & 0x7F);  
}




